﻿#ifndef BTOSGMYCAR_H
#define BTOSGMYCAR_H
#pragma execution_character_set("utf-8")
#include <osg/Material>
#include <osg/BoundingBox>
#include <btBulletCollisionCommon.h>
#include <btBulletDynamicsCommon.h>
#include <BulletDynamics/Vehicle/btRaycastVehicle.h>
#include <BulletCollision/Gimpact/btGImpactCollisionAlgorithm.h>
#include "btosg.h"

#include <osg/Material>
#include <osg/Texture2D>
#include <QtMath>

/// A four-wheeled vehicle based on btRaycastVehicle.
class btosgMyCar: public btosgObject
{
private:
    btDefaultVehicleRaycaster *rayCaster;
public:

    const std::string MYCARBODYSMALL_PATH="F:/applications/3dsmax2021/workHome/export/04carAxis/CarBody_4lun_small_new.ive";
    const std::string MYCARBODY_PATH="F:/applications/3dsmax2021/workHome/export/04carAxis/CarBody_4lun_new.ive";
    const std::string MYCARWHEEL_L1_PATH="F:/applications/3dsmax2021/workHome/export/03car/whl_l1.ive";
    //just using one
//    const std::string MYCARWHEEL_L2_PATH="F:/applications/3dsmax2021/workHome/export/03car/whl_l2.ive";
//    const std::string MYCARWHEEL_L3_PATH="F:/applications/3dsmax2021/workHome/export/03car/whl_l3.ive";
//    const std::string MYCARWHEEL_L4_PATH="F:/applications/3dsmax2021/workHome/export/03car/whl_l4.ive";
//    const std::string MYCARWHEEL_R1_PATH="F:/applications/3dsmax2021/workHome/export/03car/whl_r1.ive";
//    const std::string MYCARWHEEL_R2_PATH="F:/applications/3dsmax2021/workHome/export/03car/whl_r2.ive";
//    const std::string MYCARWHEEL_R3_PATH="F:/applications/3dsmax2021/workHome/export/03car/whl_r3.ive";
//    const std::string MYCARWHEEL_R4_PATH="F:/applications/3dsmax2021/workHome/export/03car/whl_r4.ive";
    osg::Quat modelRotation;
    btosgVec3 modelPosition;
    float dx;
    float dy;
    float dz;
    btosgVec3 dim;
    btosgVec3 up;
    btosgVec3 front;
    btRaycastVehicle *vehicle;

    //这个是小车的节点
    osg::Node *pNode;
    osg::ref_ptr<osg::PositionAttitudeTransform> wheel[4];
    double wheelRotation[4];
    double powerDegree=0;
    bool flag=false;

    void setPower(int newPower){
        if(static_cast<int>(powerDegree)>newPower){
            powerDegree-=0.5;
        }else if(static_cast<int>(powerDegree)<newPower){
            powerDegree+=0.5;
        }
        flag=true;
    }

    void slowErasePower(){
        if(flag==false){
            if(powerDegree>0){
                powerDegree-=0.05;
            }
        }
        flag=false;
//        if(static_cast<int>(powerDegree)>newPower){
//            powerDegree+=0.01;
//        }else if(static_cast<int>(powerDegree)<newPower){
//            powerDegree-=0.01;
//        }

    }


    void addPower(){
        if(powerDegree<96){
            powerDegree+=3;
        }
    }
    void subPower(){
        if(powerDegree>87){
            powerDegree-=4;
        }else if (powerDegree>76) {
            powerDegree-=3;
        }else if (powerDegree>50) {
            powerDegree-=2;
        }else if (powerDegree>0) {
            powerDegree--;
        }
    }
    std::vector<double> getEulerAngle(){
        btTransform trans;
        this->vehicle->getRigidBody()->getMotionState()->getWorldTransform(trans);
        btQuaternion quat = trans.getRotation();
        btScalar roll, pitch, yaw;
        trans.getBasis().getEulerZYX(yaw, pitch, roll);
        return std::vector<double>{yaw* 180 / M_PI,pitch* 180 / M_PI,roll* 180 / M_PI};
    }


    btosgMyCar(btosgWorld *world, btosgVec3 dimLocal = btosgVec3(2.,0.4,4.), double m=1000. ) {
         /// btosgVehicle constructor.
        btVector3 grav=world->dynamic->getGravity();
        int grav_axis=grav.minAxis();
        switch ( grav_axis ){
        case 0:
            fprintf(stderr,"Gravity direction %d (%f,%f,%f) not supported\n",grav_axis,grav[0],grav[1],grav[2]);
            break;
        case 1:
            up = btosgVec3(0., 1., 0.);
            front = btosgVec3(0., 0., 1.);
            break;
        case 2:
            up = btosgVec3(0., 0., 1.);
            front = btosgVec3(0., 1., 0.);
            break;
        }
        // dimLocal contains (Width,Height,Lenght)
        dim[0] = dimLocal[0];
        dim[1] = dimLocal[1]*up[1] + dimLocal[2]*front[1];
        dim[2] = dimLocal[2]*front[2] + dimLocal[1]*up[2];

        // dim contains (dx,dy,dz)
        dx = dim[0];
        dy = dim[1];
        dz = dim[2];


        osg::ref_ptr<osg::Geode> geo = new osg::Geode();
        osg::ref_ptr<osg::Geode> mygeo = new osg::Geode();
//        if(geo.valid()){
//            // Box to visualize the chassis
//            osg::ref_ptr<osg::Shape> sp=new osg::Box(osg::Vec3(0.,0.,0.),dx,dy,dz);
//            osg::ref_ptr<osg::Shape> cy=new osg::Cylinder(osg::Vec3(0.,0.,0.),dx,dy);
//            if(sp){
//                osg::ref_ptr<osg::ShapeDrawable> sd=new osg::ShapeDrawable(sp);
//                osg::ref_ptr<osg::ShapeDrawable> cy2=new osg::ShapeDrawable(cy);
//                if(sd){
//                    mygeo->addDrawable(sd);
////                    mygeo->addDrawable(cy2);
//                }else fprintf(stderr,"Error creating osg::Shape\n");
//            } else fprintf(stderr,"Error creating osg::Shape\n");
//        } else fprintf(stderr,"Error creating Geode\n");
        //换成我自己的方法

        //我这个让osg和bullet读取的是两个模型
        osg::Node *mbodyNode=osgDB::readNodeFile(MYCARBODY_PATH);
        osg::Group *mg=mbodyNode->asGroup()->getChild(0)->asGroup();
        int mnums=mg->getNumChildren();
        for(size_t i=0;i<mnums;i++){
            osg::Geometry *geomTmp=findGeometryNode(mg->getChild(i));

            geo->addChild(geomTmp);

        }


        osg::Node *bodyNode=osgDB::readNodeFile(MYCARBODYSMALL_PATH);
        osg::Group *g=bodyNode->asGroup()->getChild(0)->asGroup();
        int nums=g->getNumChildren();
        std::vector<osg::Geometry*> geometryArray;
        for(size_t i=0;i<nums;i++){
            osg::Geometry *geomTmp=findGeometryNode(g->getChild(i));
            geometryArray.push_back(geomTmp);

        }









        if(!model){
            model = new osg::PositionAttitudeTransform;
        }
        osg::Matrix matrix;
        osg::Matrix rotationMatrix(matrix);
        rotationMatrix(3, 0) = 0.0;
        rotationMatrix(3, 1) = 0.0;
        rotationMatrix(3, 2) = 0.0;
        osg::Quat rotation;
        rotation.set(rotationMatrix);

        model->setScale(osg::Vec3(1,1,1));
        // 设置旋转


        model->setAttitude(rotation);
        osg::Vec3 position(0,0,0);
        model->setPosition(position);




//        model->setAttitude()
        osg::PositionAttitudeTransform *pat=new osg::PositionAttitudeTransform;
        osg::Quat rotationLocal(osg::DegreesToRadians(90.0f), osg::Vec3(1.0f, 0.0f, 0.0f)); // 绕 X 轴旋转 90 度
        rotationLocal =osg::Quat(osg::DegreesToRadians(180.0f), osg::Vec3(0.0f, 0.0f, 1.0f))*rotationLocal;
        pat->setAttitude(osg::Quat(osg::DegreesToRadians(180.0f), osg::Vec3(0.0f, 0.0f, 1.0f)));
        //这轮子也要用,我都存一下
        this->modelRotation=rotationLocal;
        this->modelPosition=position;
//        pat->setScale(osg::Vec3(0.05,0.05,0.05));
        pat->setScale(osg::Vec3(0.02,0.02,0.02));
//        position=osg::Vec3(-0.137445,-1.816155,1.5f);
        position=osg::Vec3(-0.054978,-0.726462,0.6f);
        pat->setPosition(position);
        pat->addChild(geo);

        model->addChild(pat);
//        model->addChild(mygeo);
        //表示阴影投影对象
        model->setNodeMask(CastsShadowTraversalMask);
        pNode = geo;

        osg::ref_ptr<osg::Material> mat=new osg::Material;
        //设置环境光反射
        mat->setAmbient (osg::Material::FRONT_AND_BACK, osg::Vec4(0.4, 0.3, 0., 1.0));
        //设置漫反射
//        mat->setDiffuse (osg::Material::FRONT_AND_BACK, osg::Vec4(0.8, 0.7, 0.0, 1.0));
        mat->setDiffuse (osg::Material::FRONT_AND_BACK, osg::Vec4(0.8, 0.7, 1.0, 1.0));
        //设置镜面反射
        mat->setSpecular(osg::Material::FRONT_AND_BACK, osg::Vec4(0.1, 0.1, 0, 1.0));
        //设置物体的光泽度
        mat->setShininess(osg::Material::FRONT_AND_BACK, 12);
        setMaterial(mat);

        setMass(m);

        //        if (geometry) {
        //            osg::Vec3Array* vertices = dynamic_cast<osg::Vec3Array*>(geometry->getVertexArray());
        //            if (vertices) {
        //                // 调试输出顶点数量
        //                std::cout << "Number of vertices: " << vertices->size() << std::endl;

        //                // 检查每个顶点的数值
        //                for (size_t i = 0; i < vertices->size(); i++) {
        //                    const osg::Vec3& vertex = (*vertices)[i];
        //                    std::cout << "Vertex " << i << ": " << vertex.x() << ", " << vertex.y() << ", " << vertex.z() << std::endl;
        //                }
        //            } else {
        //                std::cout << "Failed to get vertex array." << std::endl;
        //            }
        //        } else {
        //            std::cout << "Failed to get geometry." << std::endl;
        //        }
        btTransform shift(btQuaternion::getIdentity(),btVector3(0.f,0.f,0.f));
        btTransform shift2(btQuaternion(pat->getAttitude().x(),pat->getAttitude().y(),pat->getAttitude().z(),pat->getAttitude().w()),btVector3(position.x(),position.y(),position.z()));


        btCollisionShape* boxShape=new btBoxShape(btVector3(dim)/2.);
        //这里我换成triangleMesh试一试

        btCompoundShape *bodyShape=new btCompoundShape();
        btTransform myShift(btQuaternion(rotationLocal.x(),rotationLocal.y(),rotationLocal.z(),rotationLocal.w()),btVector3(position.x(),position.y(),position.z()));

        for(int i=0;i<nums;i++){
            osg::Geometry *geomLocal=geometryArray[i];
            if(geomLocal){
//                osg::ref_ptr<osg::PrimitiveSet> primitiveSet = geomLocal->getPrimitiveSet(0);

////一点丁真，鉴定为三角网格
//               if (primitiveSet.valid()) {
//                          osg::PrimitiveSet::Type primitiveType = primitiveSet->getType();
//                          const char* typeName = primitiveSet->className();
//                          std::cout << "PrimitiveSet type: " << typeName << i<< std::endl;
//                          qDebug()<<"PrimitiveSet type: "<<QString(typeName)<<"   "<<i ;
//                          if(primitiveSet.get()->getMode()==osg::PrimitiveSet::TRIANGLES){
//                              qDebug()<<"mode is triangle" ;
//                          }
//                }
            osg::Vec3Array* vertices = dynamic_cast<osg::Vec3Array*>(geomLocal->getVertexArray());
//            if(vertices){
//                // 创建三角网格
//                                btTriangleMesh* triangleMesh = new btTriangleMesh();
//                                for (size_t i = 0; i < vertices->size(); i += 3) {
//                                    osg::Vec3 v1 = (*vertices)[i];
//                                    osg::Vec3 v2 = (*vertices)[i+1];
//                                    osg::Vec3 v3 = (*vertices)[i+2];
//                                    triangleMesh->addTriangle(btVector3(v1.x()*0.05, v1.y()*0.05, v1.z()*0.05),
//                                                              btVector3(v2.x()*0.05, v2.y()*0.05, v2.z()*0.05),
//                                                              btVector3(v3.x()*0.05, v3.y()*0.05, v3.z()*0.05));
//                                }

//                                // 创建三角网格形状
////                                btBvhTriangleMeshShape* triangleMeshShape = new btBvhTriangleMeshShape(triangleMesh, false);
//                                btGImpactMeshShape* triangleMeshShape = new btGImpactMeshShape(triangleMesh);
//                                if(triangleMeshShape){
//                                    std::cout<<"success construct"<< std::endl;
//                                }else{
//                                    std::cout<<"error construct"<< std::endl;
//                                }

//                                btVector3 aabbMin, aabbMax;
//                                   triangleMeshShape->getAabb(shift, aabbMin, aabbMax);

//                //                   // 输出包围盒的最小和最大坐标
//                                   std::cout<<i << "smAABB Min: " << aabbMin.x() << ", " << aabbMin.y() << ", " << aabbMin.z() << std::endl;
//                                   std::cout <<i<< "smAABB Max: " << aabbMax.x() << ", " << aabbMax.y() << ", " << aabbMax.z() << std::endl;
//                                      std::cout <<i<< "sm pointsnum: "<<vertices->size() << std::endl;


//                                bodyShape->addChildShape(shift,triangleMeshShape);

//            }else{
//                qDebug()<<"vertise is error";
//            }


//            }else {
//                qDebug()<<"geomLocal is not exist";
//            }

//            凸包
            if(vertices){

                                btConvexHullShape* triangleMeshShape = new btConvexHullShape();
                                for (size_t i = 0; i < vertices->size(); i ++) {
                                    const osg::Vec3& vertex = (*vertices)[i];


                                    triangleMeshShape->addPoint(btVector3(vertex.x()*0.02, vertex.y()*0.02, vertex.z()*0.02));

                                }


                                if(triangleMeshShape){
                                    std::cout<<"success construct"<< std::endl;
                                }else{
                                    std::cout<<"error construct"<< std::endl;
                                }

                                btVector3 aabbMin, aabbMax;
                                   triangleMeshShape->getAabb(shift2, aabbMin, aabbMax);

                //                   // 输出包围盒的最小和最大坐标
//                                   std::cout<<i << "smAABB Min: " << aabbMin.x() << ", " << aabbMin.y() << ", " << aabbMin.z() << std::endl;
//                                   std::cout <<i<< "smAABB Max: " << aabbMax.x() << ", " << aabbMax.y() << ", " << aabbMax.z() << std::endl;
//                                   std::cout <<i<< "sm pointsnum: "<<vertices->size() << std::endl;


                                bodyShape->addChildShape(shift2,triangleMeshShape);

            }else{
                qDebug()<<"vertise is error";
            }


            }else {
                qDebug()<<"geomLocal is not exist";
            }




        }
        std::cout<<"shape num:" <<bodyShape->getNumChildShapes()<< std::endl;









        // Box for collisions and center-of-gravity definition
        btCompoundShape *chassisShape=new btCompoundShape;
        //btCompoundShape是Bullet Physics库中的一个类，用于创建复合形状，即由多个子形状组合而成的形状。
//        chassisShape->addChildShape(shift,boxShape);
        //这里不能用Shift，我这里要和上面那个保持一直



        chassisShape->addChildShape(shift,bodyShape);
//        bodyShape->addChildShape(shift,boxShape);
//        btCollisionShape* CylinderShape=new btCylinderShape(btVector3(this->dim.x(),this->dim.y()/2,this->dim.x()));
//        bodyShape->addChildShape(shift,CylinderShape);

        btVector3 aabbMin, aabbMax;
           bodyShape->getAabb(shift, aabbMin, aabbMax);

           // 输出包围盒的最小和最大坐标
           std::cout << "AABB Min: " << aabbMin.x() << ", " << aabbMin.y() << ", " << aabbMin.z() << std::endl;
           std::cout << "AABB Max: " << aabbMax.x() << ", " << aabbMax.y() << ", " << aabbMax.z() << std::endl;



        shape=chassisShape;
//           shape=boxShape;
        if ( !shape ) fprintf(stderr,"Error creating btShape\n");
//        createRigidBody();

        btDefaultMotionState *mState=new btDefaultMotionState;
        btVector3 inertia(0,0,0);
        std::cout << "compound masstest: " << mass << std::endl;
//        btCollisionShape *msha=shape;
         shape->calculateLocalInertia(mass,inertia);



        std::cout << "compound masstest: " << mass << std::endl;
        std::cout << "compound inertia test: " << inertia.x()<<","<<inertia.y()<<","<<inertia.z() << std::endl;
        btRigidBody::btRigidBodyConstructionInfo cinfo=btRigidBody::btRigidBodyConstructionInfo(mass,mState,shape,inertia);
        cinfo.m_restitution=0.9f;
//        cinfo.m_friction=10.f;
        cinfo.m_friction=30.f;
        body=new btRigidBody(cinfo);

//        std::cout << "body mass: " << body->getInvMass() << std::endl;
//        if(!this->body){
//            fprintf(stderr,"Error creating btBody\n");
//        }

        //btDefaultVehicleRaycaster：默认车辆射线投射器。
//        btRaycastVehicle：射线投射车辆。
//        btVehicleTuning：车辆调整参数。
        rayCaster=new btDefaultVehicleRaycaster(world->dynamic);
        btRaycastVehicle::btVehicleTuning tuning;
        this->vehicle=new btRaycastVehicle(tuning,body,rayCaster);

        body->setActivationState(DISABLE_DEACTIVATION);
        world->dynamic->addVehicle(this->vehicle);

        //Adds the wheels to the vehicle
        //添加轮子
        //这个就相当于是车子的大致的长宽高
        btVector3 halfExtents = btVector3((aabbMax-aabbMin).x()/2.,(aabbMax-aabbMin).y()/2.,(aabbMax-aabbMin).z()/2.);
        addWheels(&halfExtents, this->vehicle, tuning);

        printf("vehicle body created\n");
        //继续
        this->vehicle->setCoordinateSystem(0,btVector3(up).maxAxis(), btVector3(front).maxAxis());

//        for (int i = 0; i < vehicle->getNumWheels(); i++) {
//            btWheelInfo& nwheel = vehicle->getWheelInfo(i);
//            nwheel.m_frictionSlip = 5;
//        }




    }

    void addWheels(
        btVector3* halfExtents,
        btRaycastVehicle* vehicle,
        btRaycastVehicle::btVehicleTuning tuning)
    {
        /**
          我先把轮胎读取进来
          */
        osg::Node *bodyNode=osgDB::readNodeFile(MYCARWHEEL_L1_PATH);
//        osg::Geometry *geometry=findGeometryNode(bodyNode);
//        geo->addChild(geometry);
        osg::Group *g=bodyNode->asGroup()->getChild(0)->asGroup();
        int nums=g->getNumChildren();
        std::vector<osg::Geometry*> geometryArray;
        osg::Geode *mgeo=new osg::Geode;
        for(size_t i=0;i<nums;i++){
            osg::Geometry *geomTmp=findGeometryNode(g->getChild(i));
            geometryArray.push_back(geomTmp);
            mgeo->addChild(geomTmp);
        }
        osg::PositionAttitudeTransform *pat=new osg::PositionAttitudeTransform;

        pat->setAttitude(this->modelRotation);
//        pat->setPosition(this->modelPosition);
//        pat->setAttitude(this->modelRotation);
//        pat->setPosition(osg::Vec3(3,-3.7495,-1.01225));
        pat->setPosition(osg::Vec3(1.2,-1.4998,-0.4));
//        pat->setScale(osg::Vec3(0.05,0.05,0.05));
        pat->setScale(osg::Vec3(0.02,0.02,0.02));
        pat->addChild(mgeo);
//        model->addChild(pat);
        osg::BoundingBox boundingBoxl1=mgeo->getBoundingBox();
        double myWheelWidth=(boundingBoxl1.xMax()-boundingBoxl1.xMin())*0.02;
        double myWheelDiameter=(boundingBoxl1.yMax()-boundingBoxl1.yMin())*0.02;
//        qDebug()<<"lunxlength: "<<(boundingBoxl1.xMax()-boundingBoxl1.xMin())*0.05;
//        qDebug()<<"lunylength: "<<(boundingBoxl1.yMax()-boundingBoxl1.yMin())*0.05;
//        qDebug()<<"lunlength: "<<(boundingBoxl1.zMax()-boundingBoxl1.zMin())*0.05;

        //掉毛，这里写右半边的轮胎,轮胎都一样，我这里就不一个一个读取了
        osg::PositionAttitudeTransform *patr=new osg::PositionAttitudeTransform;
//        patr->setPosition(osg::Vec3(-3,-3.7495,-1.01225));
        patr->setPosition(osg::Vec3(-1.2,-1.4998,0.4));
//        patr->setScale(osg::Vec3(0.05,0.05,0.05));
        patr->setScale(osg::Vec3(0.02,0.02,0.02));
        patr->addChild(mgeo);
        osg::Quat rightRotation(this->modelRotation.x(),this->modelRotation.y(),this->modelRotation.z(),this->modelRotation.w());
        rightRotation*=osg::Quat(osg::PI, osg::Vec3(0.0, 1.0, 0.0));

        patr->setAttitude(rightRotation);
//        model->addChild(patr);






//        bodyShape->getAabb(btTransform::getIdentity(), aabbMin, aabbMax);

//        // 输出包围盒的最小和最大坐标
//        std::cout << "AABB Min: " << aabbMin.x() << ", " << aabbMin.y() << ", " << aabbMin.z() << std::endl;
//        std::cout << "AABB Max: " << aabbMax.x() << ", " << aabbMax.y() << ", " << aabbMax.z() << std::endl;




        //投射方向，转轴方向，轮胎的悬挂长度，宽度，半径，连接的高度，轴的位置 重点
        /// Adds four wheels to the vehicle.

        // The direction of the raycast, the btRaycastVehicle uses raycasts
        // to sense the ground under the wheels.
        //确定轮子投射的方向
        //重点
        btVector3 wheelDirectionCS0(-btVector3(up));
        // The axis which the wheel rotates arround
//        定义了绕其旋转的轴^表示向量的叉乘操作
        btVector3 wheelAxleCS(btosgVec3(front^up));
        // center-of mass height if mass=0
        // height = suspensionRestLength-mass.g/m_suspensionStiffness
        //定义了悬挂系统的初始长度
//        btScalar suspensionRestLength(0.80);
//        btScalar suspensionRestLength(1.2);
//        btScalar suspensionRestLength(1.3);
        btScalar suspensionRestLength(0.52);
        //车轮的宽度
//        btScalar wheelWidth(0.2);
        btScalar wheelWidth(myWheelWidth);
        //车轮的半径
//        btScalar wheelRadius(0.3);
        btScalar wheelRadius(myWheelDiameter/2);
        // The height where the wheels are connected to the chassis
        //

        //连接点相对于车辆的高度位置
//        btScalar connectionHeight =0.4;
//        btScalar connectionHeight =myWheelDiameter;
        btScalar connectionHeight =0.5;

        //前后轴相距中心的位置
//        double frontAxisPos = 1.25;
//        double backAxisPos = 1.25;
//        double frontAxisPos = 3.3;
//        double backAxisPos =3.3;
        double frontAxisPos = 1.32;
        double backAxisPos =1.32;

        // All the wheel configuration assumes the vehicle is centered at the origin and a right handed coordinate system is used
        btVector3 wheelConnectPoint;
        // Adds the front wheels
        //重点，这里我也看的不是特别懂
//        wheelConnectPoint=btVector3(
//                    halfExtents->x()+wheelWidth/2,
//                    connectionHeight*up[1]+frontAxisPos*front[1],
//                    connectionHeight*up[2] + frontAxisPos*front[2]

//                    );
//        因为他这个有那个天线，所以有一些虚宽,这个宽度我是在其他的代码里面测量的
//        double boxWidth=2.75;
        double boxWidth=1.1;
        //中间的轮子
        wheelConnectPoint=btVector3(
                    boxWidth+wheelWidth/2,
                    connectionHeight*up[1]+frontAxisPos*front[1],
                connectionHeight*up[2] + frontAxisPos*front[2]

                );
        vehicle->addWheel(wheelConnectPoint,wheelDirectionCS0,wheelAxleCS,suspensionRestLength,wheelRadius,tuning,true);
        vehicle->addWheel(wheelConnectPoint*btVector3(-1, 1, 1),wheelDirectionCS0,wheelAxleCS,suspensionRestLength,wheelRadius,tuning,true);
//        //重点
//        //Adds the rear wheels
////        qDebug()<<"yi ban chang"<<halfExtents->x();
//        //第三排的轮子
        wheelConnectPoint=btVector3(
                    boxWidth+wheelWidth/2.,
                    connectionHeight*up[1] - backAxisPos*front[1],
                    connectionHeight*up[2] - backAxisPos*front[2]
                    );
        vehicle->addWheel(wheelConnectPoint,wheelDirectionCS0,wheelAxleCS,suspensionRestLength,wheelRadius,tuning,false);
        vehicle->addWheel(wheelConnectPoint*btVector3(-1, 1, 1),wheelDirectionCS0,wheelAxleCS,suspensionRestLength,wheelRadius,tuning,false);


//        //这个是创造轮子的模型,我这里换成了osg的model,暂时不需要了
//         Create one graphics wheel
//        osg::ref_ptr<osg::Geode> geo=new osg::Geode;
//        osg::ref_ptr<osg::Shape> sp;
//        //大轮子
//        sp=new osg::Cylinder(osg::Vec3(0.,0.,0.),wheelRadius,wheelWidth);
//        if(sp){
//            osg::ref_ptr<osg::ShapeDrawable> sd=new osg::ShapeDrawable(sp);
//            if(sd){
//                geo->addDrawable(sd);
//                // Setup tire material

//                osg::ref_ptr<osg::StateSet> stateset=new  osg::StateSet;
//                osg::ref_ptr<osg::Material> mat=new osg::Material;
//                //设置环境光反射
//                mat->setAmbient(osg::Material::FRONT_AND_BACK,osg::Vec4(0.,0.,0.,1.0));
//                //设置漫反射
//                mat->setDiffuse(osg::Material::FRONT_AND_BACK,osg::Vec4(0.1, 0.1, 0.1, 1.0));
//                //设置镜面反射
//                mat->setSpecular(osg::Material::FRONT_AND_BACK, osg::Vec4(0, 0, 0, 1.0));
//                //设置光泽度属性
//                mat->setShininess(osg::Material::FRONT_AND_BACK, 64);
//                //设置属性
//                stateset->setAttribute(mat, osg::StateAttribute::OVERRIDE|osg::StateAttribute::ON);
//                stateset->setTextureAttributeAndModes(0,NULL, osg::StateAttribute::OFF);

//                sd->setStateSet( stateset );


//            } else fprintf(stderr,"Error creating osg::Shape\n");
//        } else fprintf(stderr,"Error creating osg::Shape\n");

//        //里面的内胎
//        sp=new osg::Cylinder(osg::Vec3(0.,0.,0.),wheelRadius*0.75,wheelWidth+0.005);
//        if(sp){
//            osg::ref_ptr<osg::ShapeDrawable> sd=new osg::ShapeDrawable(sp);
//            if ( sd ) {
//                geo->addDrawable(sd);
//                osg::ref_ptr<osg::StateSet> stateset = new osg::StateSet();
//                // Wheel material
//                osg::ref_ptr<osg::Material> mat = new osg::Material;
//                mat->setAmbient (osg::Material::FRONT_AND_BACK, osg::Vec4(0.8, 0.8, 0.8, 1.0));
//                mat->setDiffuse (osg::Material::FRONT_AND_BACK, osg::Vec4(0.4, 0.4, 0.4, 1.0));
//                mat->setSpecular(osg::Material::FRONT_AND_BACK, osg::Vec4(0, 0, 0, 1.0));
//                mat->setShininess(osg::Material::FRONT_AND_BACK, 64);
//                stateset->setAttribute(mat, osg::StateAttribute::OVERRIDE|osg::StateAttribute::ON);
//                // Setup wheel texture
//                osg::ref_ptr<osg::Image> image = osgDB::readRefImageFile( "E:/mymachinework/osgwork/06bullet/work01_myCarWorld/wheel.jpg" );
//                if (image) {
//                    osg::Texture2D* texture = new osg::Texture2D;
//                    texture->setImage(image);
//                    texture->setFilter(osg::Texture::MIN_FILTER, osg::Texture::LINEAR);
//                    stateset->setTextureAttributeAndModes(0, texture, osg::StateAttribute::ON);
//                }
//                stateset->setMode(GL_LIGHTING, osg::StateAttribute::OVERRIDE|osg::StateAttribute::ON);
//                sd->setStateSet( stateset );

//            }   else fprintf(stderr,"Error creating osg::Shape\n");
//        } else fprintf(stderr,"Error creating osg::Shape\n");

//        osg::PositionAttitudeTransform *gen_wheel=new osg::PositionAttitudeTransform;
//        gen_wheel->setPosition(osg::Vec3(0.,0.,0));
//        gen_wheel->setAttitude(osg::Quat(-osg::PI/2.0,osg::Vec3(0.,1.,0.)));

//        gen_wheel->addChild(geo);
//        model->addChild(gen_wheel);



        printf( "num wheels %d\n",vehicle->getNumWheels());
        // Configures each wheel of our vehicle, setting its friction, damping compression, etc.
        // For more details on what each parameter does, refer to the docs

        for(int i=0;i<this->vehicle->getNumWheels();i++){
            btWheelInfo& iWheel = vehicle->getWheelInfo(i);
            // Affects rest height
            //悬挂刚度，影响下面两个阻尼
//            iWheel.m_suspensionStiffness=15.;
//            iWheel.m_suspensionStiffness=20.;
            iWheel.m_suspensionStiffness=8.;
            //压缩阻尼
            iWheel.m_wheelsDampingCompression=0.6 * 2 * btSqrt(iWheel.m_suspensionStiffness); // 0.8;
            //放松阻尼
            iWheel.m_wheelsDampingRelaxation =  0.5 * 2 * btSqrt(iWheel.m_suspensionStiffness); // 1;
            // iWheel.m_maxSuspensionForce = 150000.; // 6000
            printf("maxSupForce %f\n", iWheel.m_maxSuspensionForce);

            //Larger friction slips will result in better handling
//            iWheel.m_frictionSlip=1.5;
            iWheel.m_frictionSlip=3;
            iWheel.m_rollInfluence=1;
            wheel[i] = new osg::PositionAttitudeTransform;
            if(wheel[i]){
                //先添加的右边轮子，在添加的左边的轮子
                if(i%2==0){
                    wheel[i]->addChild(patr);

                }else {
                    wheel[i]->addChild(pat);
//                    wheel[i]->addChild(pat);
                }
//                wheel[i]->addChild(gen_wheel);
                btosgVec3 iPos = iWheel.m_chassisConnectionPointCS;
                wheel[i]->setPosition(iPos);
                printf("  Wheel %d, %f %f %f\n",i,iPos[0],iPos[1],iPos[2]);
                model->addChild( wheel[i] );
            }




        }






    }
    void printInfo() {
        /// Outputs vehicle's info.
        btosgPrint("Center of Mass", body->getCenterOfMassPosition());
        btosgPrint("Mass", mass);
        for( int i=0 ; i<vehicle->getNumWheels(); i++)
        {
            btWheelInfo& iWheel = vehicle->getWheelInfo(i);
            btosgPrint("Wheel", i);
            btosgPrint("  ChassisConnectionPoint", iWheel.m_chassisConnectionPointCS);
            btosgPrint("  WheelDirection", iWheel.m_wheelDirectionCS);
            btosgPrint("  WheelAxle", iWheel.m_wheelAxleCS);

        }
    }


    void logPosition() {
        /// Outputs vehicle's position.
        if (body) {
            btTransform wTrans;
            body->getMotionState()->getWorldTransform(wTrans);
            btVector3 pos;
            pos = wTrans.getOrigin();
            if ( name )
                std::cout << "Object   " << name << " position " << pos[0] << " " << pos[1] << " " << pos[2] << std::endl;
            else
                std::cout << "Object _NO_NAME_ position " << pos[0] << " " << pos[1] << " " << pos[2] << std::endl;
            return;
            for( int i=0 ; i<4 ; i++ ) {
                btWheelInfo& iWheel = vehicle->getWheelInfo(i);
                std::cout << "  whell " << i << ", contact: " << iWheel.m_raycastInfo.m_isInContact ;
                printf("  rotation %f\n", iWheel.m_rotation);
            }
        }
    }


    virtual void update(){
        /// Vehicle's update callback.
        /// This function is called automatically from World::setpSimulation() for each registered vehicle.
        /// Positions graphical vehicle and wheels from their physhical states.

        // updateVehicle() implemente in Bullet is Not required.
        // Vehicle dynamics are updated in stepSimulation();
        // updateVehicle requires frame_time that may not be available.
        // vehicle->updateVehicle(frame_time);

        // Visual update
        // Standard btosgObject::update() can be used.
        //这让model和坐标和刚体的坐标进行了一个同步
        btosgObject::update();

        // Update Wheels
        for(int i=0;i<vehicle->getNumWheels();i++){
            // btRaycastVehicle::updateVehicle() calls updateWheelTransform for every wheel.
            // Do not call vehicle->updateWheelTransform() here if btRaycastVehicle::updateVehicle() is called elsewhere.
            //vehicle->updateWheelTransform(i,true);
            btWheelInfo& iwheel=vehicle->getWheelInfo(i);
            if(wheel[i]){
                //chass是底盘的意思
                //m_chassisConnectionPointCS 这个是底盘连接坐标系 chassis 是底盘，connection 连接，connection连接,cs coordinate system 坐标新
                //m_suspensionLength这里获取了车轮的悬挂长度
                //m_wheelDirectionCS获取的车轮子

                btosgVec3 iPos=iwheel.m_chassisConnectionPointCS+
                        iwheel.m_raycastInfo.m_suspensionLength * iwheel.m_wheelDirectionCS;
                //


                wheel[i]->setPosition(iPos);
                btQuaternion steeringOrn(iwheel.m_wheelDirectionCS,-iwheel.m_steering);
                btMatrix3x3 steeringMat(steeringOrn);

                btQuaternion rotatingOrn(iwheel.m_wheelAxleCS,-iwheel.m_rotation);
                btMatrix3x3 rotatingMat(rotatingOrn);

                btMatrix3x3 fullMat=steeringMat*rotatingMat;
                btQuaternion fullQuat;
                fullMat.getRotation(fullQuat);
                wheel[i]->setAttitude(btosgQuat(fullQuat));


            }
        }
    }


    osg::Geometry* findGeometryNode(osg::Node* node) {
        if(node){
            if (node->asGeode()) {
                osg::Geode* geode = node->asGeode();
                for (unsigned int i = 0; i < geode->getNumDrawables(); ++i) {
//                    qDebug()<<geode->getNumDrawables()<<"hehe";
                    osg::Geometry* geometry = geode->getDrawable(i)->asGeometry();
                    if (geometry) {
                        return geometry;
                    }
                }
            }

            osg::Group* group = node->asGroup();
            if (group) {
                for (unsigned int i = 0; i < group->getNumChildren(); ++i) {
                    osg::Geometry* geometry = findGeometryNode(group->getChild(i));
                    if (geometry) {
                        return geometry;
                    }
                }
            }
        }


        return nullptr;
    }



};

#endif // BTOSGVEHICLE_H
